#include "Planner.h"

#include "basic_typedef.h"
#include <boost/foreach.hpp>
#include <CGAL/minkowski_sum_2.h>
#include <CGAL/Small_side_angle_bisector_decomposition_2.h>

using namespace std;

//#define SINGLE_ROBOT_PRM
//#define MULTI_ROBOT_PRM
#define MULTI_ROBOT_DRRT
//#define MULTI_ROBOT_HGRAPHS

#define EXPAND_ROBOTS

Planner::Planner(Scene* scene, int time, bool measure, double alpha, vector<vector<Conf>>* path, double* quality) : m_scene(scene), m_ret_path(path), m_ret_quality(quality)
{
    /*	this method extracts information regarding the scenario from the 
        gui and initializes the fields 
            m_robot_polygons
            m_obstacles
            m_start_confs, m_target_confs
            m_room
            m_robot_num
    */
    extract_scenario_from_gui();

    vector<Polygon_2> &robot_polygons = m_robot_polygons;
#ifdef EXPAND_ROBOTS
    std::cout << "[Planner]\tExtending robots";
    Polygon_2   approx_robot;
    double radius = 1;
    Point_2		p1(-radius, -radius);
    Point_2		p2(-radius, radius);
    Point_2		p3(radius, radius);
    Point_2		p4(radius, -radius);

    approx_robot.push_back(p1);
    approx_robot.push_back(p2);
    approx_robot.push_back(p3);
    approx_robot.push_back(p4);
    // Compute the Minkowski sum using the decomposition approach.
    CGAL::Small_side_angle_bisector_decomposition_2<Kernel>  ssab_decomp;

    BOOST_FOREACH(Polygon_2& robot, m_robot_polygons)
    {
        Polygon_2 expanded = minkowski_sum_2(robot, approx_robot, ssab_decomp).outer_boundary();
        m_robot_polygons_expanded.push_back(expanded);
    }
    robot_polygons = m_robot_polygons_expanded;
    std::cout << " - Done!\n";
#endif

    std::cout << "[Planner]\tInitializing planner...\n";
#ifdef SINGLE_ROBOT_PRM
    m_s_prm = new Prm(robot_polygons.front(), m_obstacles, m_start_confs.front(), m_target_confs.front(), m_room);
#endif
#ifdef MULTI_ROBOT_PRM
    m_prm = new MultiRobotPRM(robot_polygons, m_robot_num, m_obstacles, m_start_confs, m_target_confs, m_room);
#endif
#ifdef MULTI_ROBOT_DRRT
	m_drrt = new DRRT(robot_polygons, m_robot_num, m_obstacles, m_start_confs, m_target_confs, m_room);
#endif
#ifdef MULTI_ROBOT_HGRAPHS
    m_hgraph = new HGraphPlanner(robot_polygons, m_robot_num, m_obstacles, m_start_confs, m_target_confs, m_room, measure, alpha);
#endif
}

Planner::~Planner()
{
    
}

/*	This function is invoked by the GUI and is assumed to update the resulting */
void Planner::run()
{
    std::cout << "[Planner]\tPlanning...\n";
#ifdef SINGLE_ROBOT_PRM
    m_s_prm->generate_roadmap();
    vector<Conf> retval;
    m_s_prm->update_path(retval);
    vector<Conf>::iterator iter;
    for (iter = retval.begin(); iter != retval.end(); ++iter)
    {
        vector<Conf> vals;
        vals.push_back(*iter);
        m_path.push_back(vals);
    }
#endif
#ifdef MULTI_ROBOT_PRM
    m_prm->generate_roadmap();
    m_prm->update_path(m_path);
#endif
#ifdef MULTI_ROBOT_DRRT
    if (m_drrt->generate_roadmap(125, 7))
        m_drrt->update_path(m_path, 40, 7);
#endif
#ifdef MULTI_ROBOT_HGRAPHS
    m_hgraph->generate_roadmaps(3, 300, 12);
    double val = m_hgraph->update_path(m_path);
    if (m_ret_quality!=NULL)
    {
        *m_ret_quality = val; //Approximation
    }
#endif
    if (m_ret_path!=NULL)
    {
        for (vector<vector<Conf>>::iterator iter = m_path.begin(); iter != m_path.end(); ++iter)
        {
            m_ret_path->push_back(*iter);
        }
    }
    std::cout << "[Planner]\tStopping.\n\n";

    //	run this method when you finish to produce the path
    //	IMPORTANT: the result should be put in m_path
    transform_path_for_gui();
}
